function varargout = formationGUI(varargin)
% formationGUI MATLAB code for formationGUI.fig
%      formationGUI, by itself, creates a new formationGUI or raises the existing
%      singleton*.
%
%      H = formationGUI returns the handle to a new formationGUI or the handle to
%      the existing singleton*.
%
%      formationGUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in formationGUI.M with the given input arguments.
%
%      formationGUI('Property','Value',...) creates a new formationGUI or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before formationGUI_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to formationGUI_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help formationGUI

% Last Modified by GUIDE v2.5 14-May-2022 17:40:18

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @formationGUI_OpeningFcn, ...
                   'gui_OutputFcn',  @formationGUI_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before formationGUI is made visible.
function formationGUI_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global r fi
global N N2 %group number and agent number within a group
global A1 A2 
global B1 B2 
global C1 C2 
global H1 H2 
global R1 R2 
global Q1 Q2
global X1 X2 
global K11 K12 K21 K22 
global q1 q2 q3 q4 q5 q6 q
global r1 r2 r3 r4 r5 r6
global W W1 W2 W3 W4 W5 W6 P alpha gamma delta_t P2 Broken
global Tsum
global Out
global WW rr fifi
global glob out_initial 
global host_ip flag_rosinit
global pub
global sim_start
global scale
global pos_flag
global S num_V num_VR edge edge_D edeg_G
global vr
global xdyd
global ROS_ON

%% ???????
rosshutdown;
flag_rosinit = false;
host_ip='127.0.0.1';
sim_start=0;
A1=0;B1=1;C1=1;H1=1;
A2=[0 1;0 0];B2=[0 1]';C2=[1 0];H2=[2 1]';

q1=5;q2=5;q3=5;q4=5;q5=5;q6=5; %agent numbers(car+drones in a group)
q=[q1 q2 q3 q4 q5 q6];
r1=12;r2=12;r3=12;r4=12;r5=12;r6=12;

alpha=10; gamma=10;
delta_t=0.01;%????????????????delta_t=0.01s?????????delta_t=0.001s???????????0???????????????????????????
vr=5;   %virtual leader's speed
ROS_ON = 1; % 0 or 1 to enable ROS publish to gazebo (set to 0 for faster sim speed)
xdyd=[100, 100;    %target positions
    -100, 100;
    -40, -50;
    120, -120];

Tsum=20;
N=2;
N2=6;
scale=150;  %map width

W1=[0 0 0 0 0 0;
    0 0 0 1 0 1;
    0 0 0 1 0 0;
    1 0 1 0 0 0;
    0 0 0 0 0 1;
    0 1 0 0 0 0;];

W2=[0 0 0 0 0 0;
    1 0 0 0 0 0;
    0 1 0 0 0 0;
    1 0 0 0 1 0;
    0 0 0 1 0 0;
    0 0 0 1 0 0;];

W3=[0 0 0 0 0 0;
    0 0 0 0 1 0;
    0 1 0 0 0 0;
    1 0 0 0 1 0;
    0 1 0 1 0 0;
    0 0 0 0 1 0;];

W4=[0 0 0 0 0 0;
    1 0 1 0 0 0;
    0 1 0 0 1 0;
    0 0 1 0 0 0;
    0 0 1 0 0 0;
    0 1 0 0 0 0;];

W5=[0 0 0 0 0 0;
    0 0 0 0 0 1;
    1 0 0 1 0 1;
    0 0 1 0 0 0;
    0 0 0 1 0 0;
    0 0 1 0 0 0;];

W6=[0 0 0 0 0 0;
    1 0 1 0 0 0;
    0 1 0 0 0 0;
    0 0 0 0 0 1;
    0 0 0 0 0 1;
    1 0 0 0 1 0;];
 
W(:,:,1)=W1;
W(:,:,2)=W2;
W(:,:,3)=W3;
W(:,:,4)=W4;
W(:,:,5)=W5;
W(:,:,6)=W6;%??????????
WW=W;
Broken=zeros(6,6,6);


P=[ 0.9102    0.4142;
    0.4142    1.2872];

P2=1;

K11=-2; K12=[-2 -4 ];

X2=[1 0; 0 1];X1=[1 0];

U1=[0,1]; U2=[0 0];

K21=U1-K11*X1; K22=U2-K12*X2; 

R1=0;R2=1;

Q1=1/2;

Q2=[2.7500    0.5000;
    0.5000    0.3750];

r=[
12 12 12 12 12 12;
12 12 12 12 12 12;
12 12 12 12 12 12;
12 12 12 12 12 12;
];
rr=r;
fi=[
1/4*pi 1/4*pi 1/4*pi 1/4*pi 1/4*pi 1/4*pi;
3/4*pi 3/4*pi 3/4*pi 3/4*pi 3/4*pi 3/4*pi;
5/4*pi 5/4*pi 5/4*pi 5/4*pi 5/4*pi 5/4*pi;
7/4*pi 7/4*pi 7/4*pi 7/4*pi 7/4*pi 7/4*pi;
];
fifi=fi;

handles.selectcar=1;
handles.selectdrone=1;
handles.topo=1;

axes(handles.axes1);

state_initial_x1 = zeros(11,1);
state_initial_y1 = zeros(11,1);
state_initial_z1 = zeros(11,1);

state_initial_x2 = zeros(11,1);
state_initial_y2 = zeros(11,1);
state_initial_z2 = zeros(11,1);

state_initial_x3 = zeros(11,1);
state_initial_y3 = zeros(11,1);
state_initial_z3 = zeros(11,1);

state_initial_x4 = zeros(11,1);
state_initial_y4 = zeros(11,1);
state_initial_z4 = zeros(11,1);

state_initial_x5 = zeros(11,1);
state_initial_y5 = zeros(11,1);
state_initial_z5 = zeros(11,1);

state_initial_x6 = zeros(11,1);
state_initial_y6 = zeros(11,1);
state_initial_z6 = zeros(11,1);
 
state_initial=zeros(11,1,N,3);%????1

esstate_leader1=zeros(10,1);
esstate_follower1=zeros(9,1);

esstate_leader2=zeros(10,1);
esstate_follower2=zeros(9,1);

esstate_leader3=zeros(10,1);
esstate_follower3=zeros(9,1);

esstate_leader4=zeros(10,1);
esstate_follower4=zeros(9,1);

esstate_leader5=zeros(10,1);
esstate_follower5=zeros(9,1);

esstate_leader6=zeros(10,1);
esstate_follower6=zeros(9,1);

% esstate_leader=zeros(10,N,3);
% esstate_follower=zeros(10,N,3);%????3
% 
% 
% 
% out_initial_x1=zeros(6,1);
% out_initial_y1=zeros(6,1);
% out_initial_z1=zeros(6,1);
% 
% out_initial_x2=zeros(6,1);
% out_initial_y2=zeros(6,1);
% out_initial_z2=zeros(6,1);
% 
% out_initial_x3=zeros(6,1);
% out_initial_y3=zeros(6,1);
% out_initial_z3=zeros(6,1);
% 
% out_initial_x4=zeros(6,1);
% out_initial_y4=zeros(6,1);
% out_initial_z4=zeros(6,1);
% 
% out_initial_x5=zeros(6,1);
% out_initial_y5=zeros(6,1);
% out_initial_z5=zeros(6,1);
% 
% out_initial_x6=zeros(6,1);
% out_initial_y6=zeros(6,1);
% out_initial_z6=zeros(6,1);

out_initial=zeros(6,6,3);%????4



glob0_x1=[state_initial_x1;esstate_leader1;esstate_follower1];
glob0_y1=[state_initial_y1;esstate_leader1;esstate_follower1];
glob0_z1=[state_initial_z1;esstate_leader1;esstate_follower1];

glob0_x2=[state_initial_x2;esstate_leader2;esstate_follower2];
glob0_y2=[state_initial_y2;esstate_leader2;esstate_follower2];
glob0_z2=[state_initial_z2;esstate_leader2;esstate_follower2];

glob0_x3=[state_initial_x3;esstate_leader3;esstate_follower3];
glob0_y3=[state_initial_y3;esstate_leader3;esstate_follower3];
glob0_z3=[state_initial_z3;esstate_leader3;esstate_follower3];

glob0_x4=[state_initial_x4;esstate_leader4;esstate_follower4];
glob0_y4=[state_initial_y4;esstate_leader4;esstate_follower4];
glob0_z4=[state_initial_z4;esstate_leader4;esstate_follower4];

glob0_x5=[state_initial_x5;esstate_leader5;esstate_follower5];
glob0_y5=[state_initial_y5;esstate_leader5;esstate_follower5];
glob0_z5=[state_initial_z5;esstate_leader5;esstate_follower5];

glob0_x6=[state_initial_x6;esstate_leader6;esstate_follower6];
glob0_y6=[state_initial_y6;esstate_leader6;esstate_follower6];
glob0_z6=[state_initial_z6;esstate_leader6;esstate_follower6];

glob(:,1,1)=glob0_x1;
glob(:,2,1)=glob0_x2;
glob(:,3,1)=glob0_x3;
glob(:,4,1)=glob0_x4;
glob(:,5,1)=glob0_x5;
glob(:,6,1)=glob0_x6;
glob(:,1,2)=glob0_y1;
glob(:,2,2)=glob0_y2;
glob(:,3,2)=glob0_y3;
glob(:,4,2)=glob0_y4;
glob(:,5,2)=glob0_y5;
glob(:,6,2)=glob0_y6;
glob(:,1,3)=glob0_z1;
glob(:,2,3)=glob0_z2;
glob(:,3,3)=glob0_z3;
glob(:,4,3)=glob0_z4;
glob(:,5,3)=glob0_z5;
glob(:,6,3)=glob0_z6;



handles.number=0;
rrr=12;
rrrr=0.8;
dfi=2*pi/N;

for k=0:N-1
    glob(1,k+1,1)=rrr*cos(k*dfi);
    glob(1,k+1,2)=rrr*sin(k*dfi);
    glob(3,k+1,1)=rrr*cos(k*dfi);
    glob(3,k+1,2)=rrr*sin(k*dfi);
    
    for j=1:4
            glob(2*j+2,k+1,1)=(rrr-0.2)*cos(k*dfi)+rrrr*cos(j*2*pi/(N2-2));
            glob(2*j+2,k+1,2)=(rrr-0.2)*sin(k*dfi)+rrrr*sin(j*2*pi/(N2-2));
    end
    
        glob(22,k+1,1)=rrr*cos(k*dfi);
        glob(22,k+1,2)=rrr*sin(k*dfi);    
    for j=1:4
        glob(2*j+21,k+1,1)=rrr*cos(k*dfi);
        glob(2*j+21,k+1,2)=rrr*sin(k*dfi);
    end
    for j=1:5
        glob(2*j+10,k+1,1)=rrr*cos(k*dfi);
        glob(2*j+10,k+1,2)=rrr*sin(k*dfi);
    end
    for j=1:4
        glob(2*j+2,k+1,3)=2.1;
    end
end

% Choose default command line output for formationGUI
handles.output = hObject;

handles.topoline1=1;
handles.topoline2=1;
if W(handles.topoline1,handles.topoline2)
    set(handles.radiobutton1,'Value',1);
end
% Update handles structure

set(handles.edit6,'String',num2str(r(handles.selectdrone,handles.selectcar)));
set(handles.edit7,'String',num2str(-fi(handles.selectdrone,handles.selectcar)/pi));
guidata(hObject, handles);
plottopo(handles);
getH_t(handles);


% UIWAIT makes formationGUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = formationGUI_OutputFcn(hObject, eventdata, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;


% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
%clc
%clear all

global r fi
global N N2
global A1 A2 
global B1 B2 
global C1 C2 
global H1 H2 
global R1 R2 
global Q1 Q2
global X1 X2 
global K11 K12 K21 K22 
global q1 q2 q3 q4 q5 q6 q
global r1 r2 r3 r4 r5 r6
global W W1 W2 W3 W4 W5 W6 P alpha gamma delta_t P2  
global Tsum
global Out hout
global glob out_initial
global sim_start
global Out_msgs
sim_start=1;
%% ????
%rosshutdown;
tspan=0:delta_t:Tsum;
set(handles.text_console,'String','simulating...');
[Glob_last,hout,Out,Out_msgs,handles]=solveformation(@formationx1,tspan,glob,out_initial,handles);
set(handles.text_console,'String','simulating...saving results');
save('Out.mat','Out');
%save('Out_msgs.mat','Out_msgs'); % very slow
set(handles.text_console,'String','simulation finished');
guidata(hObject,handles);

% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)slider


% --- Executes on slider movement.
function slider1_Callback(hObject, eventdata, handles)
% hObject    handle to slider1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
global Out_msgs pub delta_t
guidata(hObject,handles);
t_slider=get(hObject,'Value');
set(handles.edit_simtime,'String',num2str(t_slider)); %????????edit_simtime??????????????
update_figure(t_slider,handles);%update plot figure
send(pub,Out_msgs(max(2,floor(t_slider/delta_t))));%publish ros message

function update_figure(time,handles)
global N Out delta_t;
t=floor(time/delta_t)+1;
t=max(2,t);%start plotting figure from the second pos
for number=1:N %group number
    set(handles.vr_leader(number),'xdata',Out(1,number,1,t),'ydata',Out(1,number,2,t),'zdata',Out(1,number,3,t));            
    set(handles.car(number),'xdata',Out(2,number,1,t),'ydata',Out(2,number,2,t),'zdata',Out(2,number,3,t));
    %set(handles.line(number),'X',[Out(2,number,1,i),Out(3,number,1,i)],'Y',[Out(2,number,2,i),Out(3,number,2,i)])
    for s=1:4 % update drone pos
        set(handles.drone(number,s),'xdata',Out(s+2,number,1,t),'ydata',Out(s+2,number,2,t),'zdata',Out(s+2,number,3,t));
        set(handles.dronelabell(number,s),'String',[]);%clear drone tags
    end
end
drawnow limitrate;

% --- Executes during object creation, after setting all properties.
function slider1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called
% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end



function edit_dronenum_Callback(hObject, eventdata, handles)
% hObject    handle to edit_dronenum (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of edit_dronenum as text
%        str2double(get(hObject,'String')) returns contents of edit_dronenum as a double
global N2;
data=str2num(get(hObject,'String'));
data=round(data);
set(hObject,'String',data);
if data ~= 4
    msgbox('invalid number');
    set(hObject,'String','4');
    N2=4;
elseif data<1
    msgbox('invalid number');
    set(hObject,'String','1');
    N2=1;   
else 
    N2=data+2;
end

% --- Executes during object creation, after setting all properties.
function edit_dronenum_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_dronenum (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_carnum_Callback(hObject, eventdata, handles)
% hObject    handle to edit_carnum (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global N N2 glob
data=str2num(get(hObject,'String'));
data=round(data);
set(hObject,'String',data);
if data>6
    msgbox('invalid number');
    set(hObject,'String','6');
    N=6;
elseif data<1
    msgbox('invalid number');
    set(hObject,'String','1');
    N=1;    
else
    N=data;
    
    rrr=10;
    rrrr=0.8;
    dfi=2*pi/N;
    for k=0:N-1
        glob(1,k+1,1)=rrr*cos(k*dfi);
        glob(1,k+1,2)=rrr*sin(k*dfi);
        glob(3,k+1,1)=rrr*cos(k*dfi);
        glob(3,k+1,2)=rrr*sin(k*dfi);
        for j=1:4
                glob(2*j+2,k+1,1)=(rrr-0.2)*cos(k*dfi)+rrrr*cos(j*2*pi/(N2-2));
                glob(2*j+2,k+1,2)=(rrr-0.2)*sin(k*dfi)+rrrr*sin(j*2*pi/(N2-2));
        end
        glob(22,k+1,1)=rrr*cos(k*dfi);
        glob(22,k+1,2)=rrr*sin(k*dfi);    
        for j=1:4
            glob(2*j+21,k+1,1)=rrr*cos(k*dfi);
            glob(2*j+21,k+1,2)=rrr*sin(k*dfi);
        end
        for j=1:5
            glob(2*j+10,k+1,1)=rrr*cos(k*dfi);
            glob(2*j+10,k+1,2)=rrr*sin(k*dfi);
        end
        for j=1:4
            glob(2*j+2,k+1,3)=2.1;
        end
    end
end
guidata(hObject,handles);
% Hints: get(hObject,'String') returns contents of edit_carnum as text
%        str2double(get(hObject,'String')) returns contents of edit_carnum as a double


% --- Executes during object creation, after setting all properties.
function edit_carnum_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_carnum (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_simtime_Callback(hObject, eventdata, handles)
% hObject    handle to edit_simtime (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_simtime as text
%        str2double(get(hObject,'String')) returns contents of edit_simtime as a double
global Tsum pub Out_msgs delta_t
simtime = str2num(get(hObject,'string'));
if simtime<=Tsum && simtime>=0
    set(handles.slider1,'Value',simtime);%set time slider time
    update_figure(simtime,handles);%update plot figure
    send(pub,Out_msgs(max(2,floor(simtime/delta_t))));
else
    msgbox({'invalid value'; ['should be less than ',num2str(Tsum),' and positive']},'warning','warn'); %error message window
end

% --- Executes during object creation, after setting all properties.
function edit_simtime_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_simtime (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_tsum_Callback(hObject, eventdata, handles)
% hObject    handle to edit_tsum (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global Tsum
Tsum=str2num(get(hObject,'String'));
set(handles.slider1,'Max',Tsum,'Min',0,'Value',0);%reset time slider threshold
set(handles.edit_simtime,'String','0'); %????????edit_simtime??????????????
guidata(hObject,handles);
% Hints: get(hObject,'String') returns contents of edit_tsum as text
%        str2double(get(hObject,'String')) returns contents of edit_tsum as a double


% --- Executes during object creation, after setting all properties.
function edit_tsum_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_tsum (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
uiwait;


% --- Executes on button press in pushbutton3.
function pushbutton3_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
uiresume;


% --- Executes on selection change in popupmenu1.
function popupmenu1_Callback(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
global r fi
handles.selectcar=get(hObject,'Value');
handles.topo=handles.selectcar;
set(handles.edit6,'String',num2str(r(handles.selectdrone,handles.selectcar)));
set(handles.edit7,'String',num2str(-fi(handles.selectdrone,handles.selectcar)/pi));
guidata(hObject,handles);
plottopo(handles);
getH_t(handles);

global W Broken
if W(handles.topoline1,handles.topoline2,handles.topo)
    set(handles.radiobutton1,'Value',1);
else
     set(handles.radiobutton1,'Value',0);
end
set(handles.ifbroken,'Value',Broken(handles.topoline1,handles.topoline2,handles.topo));

% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu1 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu1


% --- Executes during object creation, after setting all properties.
function popupmenu1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on selection change in popupmenu4.

function popupmenu4_Callback(hObject, eventdata, handles)
global r fi
% hObject    handle to popupmenu4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
handles.selectdrone=get(hObject,'Value');
set(handles.edit6,'String',num2str(r(handles.selectdrone,handles.selectcar)));
set(handles.edit7,'String',num2str(-fi(handles.selectdrone,handles.selectcar)/pi));
guidata(hObject,handles);
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu4 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu4


% --- Executes during object creation, after setting all properties.
function popupmenu4_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit7_Callback(hObject, eventdata, handles)
% hObject    handle to edit7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
global fi
fi(handles.selectdrone,handles.selectcar)=-str2num(get(hObject,'String'))*pi;
guidata(hObject,handles);
plottopo(handles);

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit7 as text
%        str2double(get(hObject,'String')) returns contents of edit7 as a double


% --- Executes during object creation, after setting all properties.
function edit7_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit6_Callback(hObject, eventdata, handles)
% hObject    handle to edit6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global r
r(handles.selectdrone,handles.selectcar)=str2num(get(hObject,'String'));
guidata(hObject,handles);
plottopo(handles);
% Hints: get(hObject,'String') returns contents of edit6 as text
%        str2double(get(hObject,'String')) returns contents of edit6 as a double


% --- Executes during object creation, after setting all properties.
function edit6_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit8_Callback(hObject, eventdata, handles)
% hObject    handle to edit8 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB

handles.topo=str2num(get(hObject,'String'));
guidata(hObject,handles);
plottopo(handles);


function plottopo(handles)

global r fi W
global S num_V num_VR edge edge_D edeg_G

axes(handles.axes2);
cla reset;
topo=handles.topo;
X=zeros(6,1);
Y=zeros(6,1);
Z=zeros(6,1);
for i=1:4
    X(i+2,1)=-r(i,topo)*cos(fi(i,topo));
    Y(i+2,1)=-r(i,topo)*sin(fi(i,topo));
    
end
Z=[-1 0 5 5 5 5]';
position=1;

for i=1:6
    for j=1:6
        if (W(i,j,topo)==1)
            s1(position)=j;
            t1(position)=i;
            position=position+1;
        end
    end
end

G1=digraph(s1,t1);
plot(G1,'XData',X,'YData',Y,'ZData',Z);
view(30,25);

axes(handles.axes1);



% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit8 as text
%        str2double(get(hObject,'String')) returns contents of edit8 as a double


% --- Executes during object creation, after setting all properties.

function edit8_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in pushbutton4.
function pushbutton4_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
axes(handles.axes1);
cla reset;
% handles    structure with handles and user data (see GUIDATA)


% --- Executes on button press in pushbutton5.
function pushbutton5_Callback(hObject, eventdata, handles)
global r rr W WW fi fifi
r=rr;
W=WW;
fi=fifi;
set(handles.edit6,'String',num2str(r(handles.selectdrone,handles.selectcar)));
set(handles.edit7,'String',num2str(-fi(handles.selectdrone,handles.selectcar)/pi));

if W(handles.topoline1,handles.topoline2,handles.topo)
    set(handles.radiobutton1,'Value',1);
else
     set(handles.radiobutton1,'Value',0);
end
plottopo(handles);

%[handles]=findline(handles);
% hObject    handle to pushbutton5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --- Executes on selection change in popupmenu5.
function popupmenu5_Callback(hObject, eventdata, handles)
% hObject    handle to popupmenu5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
global W Broken
handles.topoline1=get(hObject,'Value');
if W(handles.topoline1,handles.topoline2,handles.topo)
    set(handles.radiobutton1,'Value',1);
else
     set(handles.radiobutton1,'Value',0);
end
set(handles.ifbroken,'Value',Broken(handles.topoline1,handles.topoline2,handles.topo));
guidata(hObject,handles);
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu5 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu5


% --- Executes during object creation, after setting all properties.
function popupmenu5_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on selection change in popupmenu6.
function popupmenu6_Callback(hObject, eventdata, handles)
% hObject    handle to popupmenu6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
global W Broken
handles.topoline2=get(hObject,'Value');
if W(handles.topoline1,handles.topoline2,handles.topo)
    set(handles.radiobutton1,'Value',1);
else
     set(handles.radiobutton1,'Value',0);
end
set(handles.ifbroken,'Value',Broken(handles.topoline1,handles.topoline2,handles.topo));
guidata(hObject,handles);
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu6 contents as cell array
%        contents{get(hObject,'Value')} returns selected item from popupmenu6


% --- Executes during object creation, after setting all properties.
function popupmenu6_CreateFcn(hObject, eventdata, handles)
% hObject    handle to popupmenu6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in radiobutton1.
function radiobutton1_Callback(hObject, eventdata, handles)
% hObject    handle to radiobutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
global W

y=get(hObject,'Value');
W(handles.topoline1,handles.topoline2,handles.topo)=y;
plottopo(handles);
guidata(hObject,handles);
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of radiobutton1


% --- Executes on button press in pushbutton6.
function pushbutton6_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
errors();
search_error();


% --- Executes when figure1 is resized.
function figure1_SizeChangedFcn(hObject, eventdata, handles)
% hObject    handle to figure1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --- Executes on button press in pushbutton_loadmodel.
function pushbutton_loadmodel_Callback(hObject, eventdata, handles)
% ros publish inital states to launch model in gazebo
global glob sim_start N N2 host_ip flag_rosinit pub
if ~flag_rosinit
    if host_ip == '127.0.0.1'
        rosinit;%local
    else
        rosinit(host_ip);
    end
    flag_rosinit=true;
end
topic_name='/gazebo/set_model_states';
msg_type='gazebo_msgs/ModelStates'; 
pub = rospublisher(topic_name,msg_type);

ALL=rosmessage(msg_type);%ros publish message         
for j=1:N
    A=strcat('car', int2str(j-1));
    ALL.Name=[ALL.Name;A];
    for k=3:N2
        A=strcat('drone', int2str((j-1)*(N2-2)+k-3));
        ALL.Name=[ALL.Name;A];
    end
end

while 1
    if sim_start %loop until sim starts
      %rosshutdown;  
      break
    end
    for number=1:N
        yaw=(number-1)/N*2*pi; %?????j
        q = angle2quat(yaw,0, 0, 'ZYX');  %???????????????????????????????????

        team_pose=rosmessage('geometry_msgs/Pose');      %????????xyz????????????
        team_twist=rosmessage('geometry_msgs/Twist');    %????????0???????????                

        team_pose.Orientation.X=q(2);
        team_pose.Orientation.Y=q(3);
        team_pose.Orientation.Z=q(4);
        team_pose.Orientation.W=q(1);

        team_pose.Position.X=glob(3,number,1);
        team_pose.Position.Y=glob(3,number,2);
        team_pose.Position.Z=glob(3,number,3);

        ALL.Pose((number-1)*(N2-1)+1)=team_pose;
        ALL.Twist((number-1)*(N2-1)+1)= team_twist;

        for s=3:N2
            team_pose=rosmessage('geometry_msgs/Pose');    
            team_twist=rosmessage('geometry_msgs/Twist');                  

            team_pose.Orientation.X=q(2);
            team_pose.Orientation.Y=q(3);
            team_pose.Orientation.Z=q(4);
            team_pose.Orientation.W=q(1);

            team_pose.Position.X=glob(2*s-2,number,1);
            team_pose.Position.Y=glob(2*s-2,number,2);
            team_pose.Position.Z=glob(2*s-2,number,3);

            ALL.Pose((number-1)*(N2-1)+s-1)=team_pose; 
            ALL.Twist((number-1)*(N2-1)+s-1)= team_twist;
        end      
    end
    send(pub,ALL);
    pause(2);
end


% --- Executes on button press in togglebutton_play.
function togglebutton_play_Callback(hObject, eventdata, handles)
% hObject    handle to togglebutton_play (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of togglebutton_play
global Out_msgs delta_t Tsum
global host_ip flag_rosinit
if ~flag_rosinit
    if host_ip == '127.0.0.1'
        rosinit;%local
    else
        rosinit(host_ip);
    end
    flag_rosinit = true;
end
topic_name='/gazebo/set_model_states';
msg_type='gazebo_msgs/ModelStates'; 
pub = rospublisher(topic_name,msg_type);

play_state = get(hObject,'Value');
t_slider = get(handles.slider1,'Value');
t_play = (floor(t_slider/delta_t)+2):1:floor(Tsum/delta_t);
T_draw=0.2/delta_t; % draw plot interval
T_rospub=0.1/delta_t; % draw plot interval
if play_state == 1
    t0=cputime;%????????
    for i = t_play %playback data          
        if get(hObject,'Value') == 0
            break;
        end
        t_slider = i*delta_t; %play time
        if ~mod(i,T_rospub)
            set(handles.slider1,'Value',t_slider);
            set(handles.edit_simtime,'String',num2str(t_slider));
            update_figure(t_slider,handles);%update plot figure
            
            send(pub,Out_msgs(i));%publish ros message
            t_pause=(T_rospub*delta_t-(cputime-t0));
            pause(t_pause);      
            t0=cputime;
        end
    end
    set(hObject,'Value',0);
    %rosshutdown;
elseif play_state == 0
    %rosshutdown;
end
       

function edit_hostip_Callback(hObject, eventdata, handles)
% hObject    handle to edit_hostip (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_hostip as text
%        str2double(get(hObject,'String')) returns contents of edit_hostip as a double
global host_ip;
host_ip=get(hObject,'String');
msgbox(['You have changed hostIP to ',host_ip]);

% --- Executes during object creation, after setting all properties.
function edit_hostip_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_hostip (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes during object deletion, before destroying properties.
function figure1_DeleteFcn(hObject, eventdata, handles)
% hObject    handle to figure1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
rosshutdown;%rosshutdown before exit
clear global;%clear global parameters before exit. important!


% --- Executes on button press in topo_rebuild.
function topo_rebuild_Callback(hObject, eventdata, handles)
global S num_V num_VR edge edge_D edeg_G W
getH_t(handles);
weight_n=sum(edge_D(:,3))+1;
root=num_V+1;
temper=[(root)*ones(num_V,1)  [1:num_V]' weight_n*ones(num_V,1)];
edge_DR=[edge_D;temper;temper];
[pos] = ZhuliuMST(edge_DR,root,num_VR);
T=edge_DR([pos],:);
for i=1:num_VR
    if T(i,1) == num_VR
        T(i,:) = [];
        break;
    end
end
X1=[S(:,1); 1000];  Y1=[S(:,2) ;1500];  Z=[S(:,3) ;1000];
X2=S(:,1);  Y2=S(:,2);  Z2=S(:,3);
axes(handles.axes2);
cla reset;

%%%% 最小生成树（根据位置画）
s1=T(:,1);  t1=T(:,2);  weight1=T(:,3); G1=digraph(s1,t1);
%plot(G1,'XData',X2,'YData',Y2,'ZData',Z2)
view(30,25);

for i=1:6
    for j=1:6
        W(i,j,handles.topo)=0;
    end
end

for i=1:length(s1)
    W(t1(i),s1(i),handles.topo)=1;
end
plottopo(handles);
%{
%%%% 最小生成树（只有拓扑关系，无位置关系）
figure(2);
s1=T(:,1);  t1=T(:,2);  weight1=T(:,3); G1=digraph(s1,t1,weight1);
plot(G1,'EdgeLabel',G1.Edges.Weight)

%%%% 节点位置
figure(3);
s1=T(:,1);  t1=T(:,2);  weight1=T(:,3);
plot3(X2,Y2,Z2,'r.')
%}
axes(handles.axes1);

% hObject    handle to topo_rebuild (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --- Executes on button press in ifbroken.
function ifbroken_Callback(hObject, eventdata, handles)
global Broken;
global S num_V num_VR edge edge_D edeg_G
getH_t(handles);
y=get(hObject,'Value');
Broken(handles.topoline1,handles.topoline2,handles.topo)=y;
Broken(handles.topoline2,handles.topoline1,handles.topo)=y;


node_1 = handles.topoline1;
node_2 = handles.topoline2;
delete_flag_1 = (edge_D(:,1)==node_1) & (edge_D(:,2) == node_2);
delete_flag_2 = (edge_D(:,1)==node_2) & (edge_D(:,2) == node_1);
delete_flag = delete_flag_1 | delete_flag_2;
edge_D = edge_D(~delete_flag,:);

global W

y=1-get(hObject,'Value');

if(y==0)
W(handles.topoline1,handles.topoline2,handles.topo)=y;
plottopo(handles);

set(handles.radiobutton1,'Value',W(handles.topoline1,handles.topoline2,handles.topo));
end
guidata(hObject,handles);

% hObject    handle to ifbroken (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of ifbroken
function getH_t(handles)
global S num_V num_VR edge edge_D edeg_G
global r fi W S Broken
topo=handles.topo;
S=zeros(6,3);
for i=1:4
    S(i+2,1)=-r(i,topo)*cos(fi(i,topo));
    S(i+2,2)=-r(i,topo)*sin(fi(i,topo));
    S(:,3)=[0 0 5 5 5 5];
end
num_V=size(S,1);    % 节点数目
num_VR=num_V+1;

[edge_D,edeg_G,edge] = EdgeDandUn(S,1600);

for i=1:6
    for j=1:6
        if (Broken(i,j,topo)==1)
            node_1 = i;
            node_2 = j;
            delete_flag_1 = (edge_D(:,1)==node_1) & (edge_D(:,2) == node_2);
            delete_flag_2 = (edge_D(:,1)==node_2) & (edge_D(:,2) == node_1);
            delete_flag = delete_flag_1 | delete_flag_2;
            edge_D = edge_D(~delete_flag,:);
        end
    end
end


% --- Executes on button press in pushbutton16.
function pushbutton16_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton16 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.text_console,'String','trajectory planning...');
figure
% plot([1,2],[1,2])
ACO();


% --- Executes on button press in pushbutton17.
function pushbutton17_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton17 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
figure
search();
